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Suppose the beacons coordinates and their respective heights hi, h2, h3 are known; suppose the values 
r2, r3 are correctly read from the IR-ultrasonic device and transformed to d1 , d2, d3; using these equation 
the 2 solution-points being the intersections of the first two circles should be found easily. The computer 
must then simply check, which of the solutions fulfills the equation of the remaining circle. If any error 
occurs, the robot must find its position by dead-reckoning. 



The direction and the speed can easily be deduced by comparing former positions to the actual location. 
The instant position must be concluded from the rotation sensor readings and the motor-actions. 

Echo values must be pruned. They occur specially if there are obstacles which deviate the direct wave, or 
the robot is out of the beam-angle. They are recognizable in most of the cases, because they are much 
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larger than the current values. Tl^fobot may change its position only (Jfciin a certain range depending on 
its velocity. For example: 

v = 1 m/sec 

=> Amax = v * t = 1m, if t = 1 sec 

so for any d1, d2, d3, the respective increase may not be greater than 1 meter after a travel of 1 sec. This 
course, has to be adjusted to the robots real velocity. 

The beacon3bis.c program shows that the out-of-range error is recognizable by the result 0. A zero distan 
will never be possible, so this number must be an error. A problem occurs if there is no IR-reception. The 
reiceiver simply waits for signal. There is no value-transmission to the robot anymore. In such a case, the 
beacon3bis.c holds the old value. This situation will only be detectable through dead-reckoning which 
demonstrate that the actual beacon-deduced position must be erronous. 




x 



Another problem has to be considered. We have already mentioned that the communication protocole 
between one beacon and the robot needs about 150 ms. If the robot moves at a speed of 1 m/sec, its 
location at beacon2-PING (=t2) will be 15cm from the position at beacon 1 -PING (=t1). So the positioning 
algorithme has to take care to find d2 from d'2. Knowing p = v * (t2-t1) = v * 150ms, the angle 0 (which is 
direction of motion) and p 2 (see turnable beacons section), d2 can be defined through the 
cosine-triangle-formula. The same should be done for d3. 

Of course, these calculations may be omitted if the robot's velocity <= 0.25 m/sec. In this case the robot w 
not move more than 4 cm which is the standard-precision of the device. 

Obviously we'll need performing trigonometry-approximations in the positioning-task. Here the CORDIC- 
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functions sine, cosine, tan and ai4fci to be implanted in the legOS-pro^m. For perfect understanding, 
have a look at our CORDIC arctan-page . Note that the CORDIC algorithm presents the characteristics to 
give the sine and cosine together at the same time. This might be useful during way-integration, where 
these values are needed simultanously. Unlike the arctan-algorithm, the sin/cosine-algorithm decides the 
sign of kj by comparing the phase of the rotating vector to the function-argument. After the procedure, the 

real component corresponds to the cosine, whereas the imaginary component represents the sine. Both 
values have of course to be devided by the CORDIC-gain. 



/*The trig-functions sine, cosine, tan, atan, 8-digits precise*/ 
/* AUTHOR : Claude Baumann 1/5/2001, test-version*/ 



iinclude <conio.h> 
#include <unistd.h> 
#include <dbutton.h> 
#include <dkey . h> 



#define CORDIC_gain 1.646760258 



double Phase {int k) /*in degrees*/ 



switch (k) 
{ 

case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
case 
default: 



1: return 45.0000000; 

2: return 26.5650512; 

3: return 14.0362435; 

4: return 7.12501635; 

5: return 3.57633437; 

6: return 1.78991061; 

7: return 0.89517371; 

8: return 0.44761417; 

9: return 0.22381050; 

10: return 0.11190568; 

11: return 0.05595289; 

12: return 0.02797645; 

13: return 0.01398823; 

14: return 0.00699411; 

15: return 0.00349706; 

16: return 0.00174853; 

17: return 0.00087426; 

18: return 0.00043713; 

19: return 0.00021857; 

20: return 0.00010928; 

21: return 0.00005464; 

22: return 0.00002732; 

23: return 0.00001366; 

24: return 0.00000683; 

25: return 0.00000341; 

26: return 0.00000177; 

27: return 0.00000085; 

28: return 0.00000042; 

29: return 0.00000021; 

30: return 0.00000010; 

31: return 0.00000005; 

32: return 0.00000002; 

33: return 0.00000001; 
return 888; /*to silence the compiler*/ 



double trig (double alpha, int idx) 
I double a=l; 

double b=0; 

double kL=l; 

double Theta-0; 

double Temp, beta, tan; 
int L,sign,c; 



beta=alpha; 
c=0; 

while (beta>180) beta-=360; /*beta must be smaller than 180° */ 
while (beta<-180) beta+=360; /*beta must be greater than -180° */ 
if(beta>90 ) { beta=180-beta; c»l;> /*quadrant II +/ 
if(beta<-90) { beta=-beta-180; c=l;} /*quadrant III +/ 
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for <L=1;L<34;L++) 

{ if (Theta<beta) { sign=l; } else { sign=-l; } /*compare phase to argument*/ 
Theta+=sign*Phase (L) ; 
Temp^a ; 
a-=b*kL*sign; 
b+=Temp*kL*sign; 
kL/=2; 

) 

a/=CORDIC_gain; /*a=cos (alpha) */ 
b/=CORDIC_gain; /*b=sin (alpha) */ 
if (c-=l) a--a; /*quadrant matching*/ 

if(a!=0) {tan=b/a;} else { if(b>0) ( tan=999999; } else ( tan=-999999; } ) 
switch (idx) 
{ 

case 1: return a; /*give the cosine */ 

case 2: return b; /* give the sine */ 

case 3: return tan; /*give the tan */ 
default: return 888; /*to silence the compiler*/ 
J 

) 

double sin (double alpha) 

return trig (alpha, 2 ) ; 

double cos (double alpha) 

r etu rn t r i g ( alpha , 1 ) ; 

double tan (alpha) 

return trig (alpha, 3) ; 

double atan (double x) /* -90 < x < 90 */ 
{ double a, b, Temp; 

double kL=l; 

double Theta=90; 

int L,sign; 

if(x>0) {b=l;} else (b=-l; } 

Theta*=-b; /*starting value +90 or -90 depending sign of x*/ 
a=x*b; /*a will always be positive */ 

Temp=a; /*now rotate by +/- 90°; this could be done more quickly, but for better understanding*/ 
a=b*b; 
b--Temp*b; 

for (L=1;L<34;L++) 
{ 

if(b>0) { sign— 1; } else { sign=l; } 
Theta+=sign*Phase (L) ; 
Temp=a ; 
a-=b*kL*sign; 
b+=Temp*kL*sign; 
kL/=2; 

J 

if{x>0) { return Theta+90;} else {return Theta-90;) 

} 

int main() /*test program*/ 



int j ; 
int p; 

for ( j=-10; j<ll; 
( 

cputs ("x") ; 
sleep (1) ; 
lcd_int (j) ; 
sleep (1 ) ; 



cputs ( "atan" ) ; 
sleep (1) ; 
p=atan( j) ; 
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olcd_int (p) ; 

while (RELEASED (dbut ton { ) , BUTTON_VIEW) ) ; /+wait until view-button pressed*/ 

) 

return 0; 

} 



Let bj(Xj , yj) be the respective coordinates of the beacons and p r (x r , y r ) the position of the robot. 
bj(Xj , yj) are stored in the 4 RCXs as constants when downloading the programs. 

Since the ultrasonic-reflector permits horizontal reception, we may suppose the beacons-positions over 
ground hj are all equal to zero, cf Preliminary page 

==> d = r 
i 'i 

Here now the TEST beacon-positioning-procedure, (pruning echo values is still omitted; we use only a slo 
motion-robot so that speed <=0.25 m/sec): 

At the beginning the program must initialize some important values which are needed all over the repeate 
positionings: 

• define the macro SQR(a) ((a)*(b)) 

• define a constant TOLERANCE =-0 

• include a squareroot-approx. SQRT(x) 

• include an abs-value function ABS(x) 

• include the trig-functions 

• Find the two beacons for which the difference ABSty - yp is maximized and non-zero. Let's call them 
b m and b n . 

• this may be done through the following lines: 



max:=0; 
for i:=l to 2 do 
for j:=2 to 3 do 
begin 
diff:=abs(y[i]-y[j]); 

if diffi>max then begin max:=diff; m:=i; n:=j; k:=6-i-j;*end; 
end; 

Note that the k:=6-i-j is a simple trick: 
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□DOPE 



• Calculate: diff = y m - y n 

• Calc: B = (x m -x n )/diff 

• Calc: u = (1 + SQR(B)) 

in order to avoid ambiguity, a, b, c of the formulas above are replaced by u,v,w 
Note that u is never zero 

• repeat forever the fixing of the exact position of the robot: 

o execute a PING-session and store the values r ( 

o Calc: A = (SQR(d n ) - SQR(d m ) + SQR(x m ) - SQR(x n )) / diff / 2 + (y m + y n ) / 2 
o Calc: v = (2*y n *B - 2*x n - 2*A*B) 

o Calc: w = (-SQR(d n ) + SQR(x n ) + SQR(y n ) + SQR(A) - 2*y n *A) 
o Calc: delta = SQR(v) - 4*u*w 

o if delta < 0 ==> error Note that the program must respect a certain tolerance due to calculation-error an 

the 4.5 cm device-tolerance. In these cases let delta = 0 
o if delta = 0 ==> 1 solution x r = - u / v / 2 

o if delta > 0 ==> store the two solutions: 

■ sq_d = SQRT(delta) 

- x r1 = (- v + sq_d) / u / 2 ; y r1 = A - B*x r1 
" x r2 = (" v " sc l- c ' V u ' 2 i Yr2 = A " B * x r2 

■ Calc: tempi = SQR(x k - x f1 ) + SQR(y k - y M ) - SQR(d k ) (b k is the remaining beacon) 
m Calc: temp2 = SQR(x k - x^) + SQR(y k - y^) - SQR(d k ) 

■ if ABS( tempi ) < ( temp2 ), (x r1 ,y r1 ) is the solution else it is (x^.y^) 

- check if ABS(temp so|utjon )=-0 +/- TOLERANCE, otherwise error 

o convert the position data to a two-byte message which is sent to the beacons with adding the 
prefix P 

o the beacons must now calculate the angles betaj = 90 + arctan( - y r ) / (Xj - x f ) ) and adjust th 
heading accordingly using the rotation sensor, (consider the cases where (Xj = x r ) The best wo 
be to work only with values between -1 and 1 for the arctan-function.) 

Here another test program. It searches and displays the values x r1 , y r1 , x^ and y^m. It works already very 
well. Note the changes in the receive-function. THIS PROGRAM REC_BOT6.c SHOULD BE USED TO 
TEST THE BEACON-ROBOT COMMUNICATION AND THE POSITIONING. Put attention to enter the 
correct beacon-coordinates in the blue marked line. 



/*rec_bot6 . c : should send a message to each beacon in order to PING 
Then calculate and display the values 
NEW NEW NEW positioning-calculations*/ 

#include <unistd.h> 
#include <conio.h> 
#include <dmotor.h> 
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#include <dsensor.h> 
#include <string.h> 
#include <lnp.h> 
#include <lnp-logical .h> 

#define aa .0710843 /^linear function to transform raw_l to real half-byte*/ 
#define bb -6.9802983 

#define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ 

int LOW_read,HIGH_read; 

#define POINTS 3 
#define SOLUTIONS 2 

int xfPOINTS] ={0,92,0} ; /*make these variables global*/ 
int y [POINTS] ={0,75, 150} ; /*beacon-positions in cm*/ 
int d[ POINTS] = {0,0,0} ; 
int i, j , k,m, n, dif f ,max; 
double B,u; 

int receive {) 
{ 

int idx,wdt,err; 
long int SUM; 



/♦initialize main variables*/ 

LOW_read=0; 

HIGH_read=0; 

err-0; /*no error*/ 

wdt=0; /*watch-dog-timer*/ 

/*communication-protocole*/ 



while (SENSOR_1>16000) /*wait until start-message*/ 
{ 

wdt+=l; 

if (wdt>3000) { err-+l; break; } 

} 

if (err==0) /*continue if no error*/ 
{ 

wdt=0 ; 

while (SENS0R_I< 16001) /*wait until lowjbyte*/ 
{ 

wdt+=l ; 

if (wdt>1000) { err=+2; break; } 

} 

} 

/*while waiting until pause-message compute average*/ 
if{err-«0) /^continue if no error*/ 

{ 

SUM=0; 
idx=0; 

while (SENS0R_1>1 6000) /*wait until high_byte*/ 
{ 

SUM+=SENSOR_l; 
idx+=l; 

if(idx>1000) { err=+3; break; } 

} 



LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 

} 



if (err==0) /*continue if no error*/ 
{ 

wdt=0; 

while (SENSOR_1<16001) 
{ 

wdt+=l ; 

if (wdt>1000) { err=+4; break; } 

} 

} 

/*while waiting until stop-message compute average*/ 

if(err==0) /*continue if no error*/ 
{ 

SUM=0; 
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idx^O; 

while <SENSOR_1>16000) 
{ 

SUM+=SENS0R_1; 
idx+=l; 

if (idx>1000) { err=+5; break; ) 

) 



HIGH_read~SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 

) 



if (err==0) /*continue if no error*/ 
{ 

wdt^O; 

while (SENSOR_1<16001) /*wait until NO_SIGNAL condition*/ 
{ 

wdt+=l; 

if (wdt>1000) { err=+6; break; } 

) 

} 

return err; 

} 



int distance (int L, int H) 
{ 

double raw_l , dist ; 
int LOW, HIGH, result; 
/^convert raw sensor-values to half -byte values*/ 
raw_l=100000/L; 
raw_l=aa * r aw_l+bb ; 
LOW=raw_l; 
raw_l=100000/H; 
r aw_l-aa * raw_l +bb ; 
HIGH=raw_l; 

/*calculate the distance*/ 
dist=cycle* (16*HIGH+LOW) +. 5; 
result=dist; /*double to integer*/ 

return (dist) ; 



/*++++++ ++++++++++■+++*/ 



double abs (double x) /*abs value-function*/ 
{ 

if(x>0) {return x; } else { return -x; } 

} 



double sqr (double x) /*square-function*/ 
{ 

return x*x; 

} 

double sqrt (double x) /*square-root-function*/ 

{ /*Heron's algorithm*/ 

int idx; 
double q, y; 
q=0; 
y-2; 

for (idx=0; idx<14; idx++) /*this precision is OK, for-loop prevents RCX from 

getting lost in loop*/ 

{ 

q=x/y; 
y=(y+q) /2; 

} 

return y; 



int pos() 
t 

double xr [SOLUTIONS] ; 
double yr [SOLUTIONS] ; 
int tempi, err; 
double A, v, w, sq_d, delta; 
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err=0; /*no error yet*/ 



/*now the calculations*/ 

A=( (sqr (d[n] )-sqr(d(mJ )+3qr(x[m] )-sqr (x[n] ) ) /dif f+y [m] +y [n] ) /2; 



v=2* (y[n) *B-x[n]-A*B) ; 

w=-sqr (d[n] )+sqr(x[n] )+sqr (y[n] ) +sqr (A) -2*y[n] *A; 
delta=-4*u*w+sqr (v) ; 
if(delta<0) {err=7; } else 
{ 

sq_d=sqrt (delta) ; 
xr[0]=(sq_d-v) /u/2; 
xr [!]=-{ v+sq_d)/u/2 ; 
yr[0]=A-B*xr [0] ; 
yr[l]=A-B*xr [1] ; 



templ=xr [0J ; 
cputs ("xrO") ; 
sleep (1) ; 
lcd_int (tempi) ; 
sleep (1) ; 



templ=yr [0] ; 
cputs ("yrO" ) ; 
sleep (1) ; 
lcd_int (tempi) ; 
sleep (1) ; 



templ=xr [1] ; 
cputs ( "xrl" ) ; 
sleep (1) ; 
lcd_int ( tempi ) ; 
sleep (1) ; 



templ=yr (1] ; 
cputs ("yrl" ) ; 
sleep (1) ; 
lcd^int (tempi) ; 
sleep (1) ; 

) 

return err; 



/*+++++++++++++++++++++++++++++++++^ 



/*cccccccccccccccccccc^^ 

int main(int argc, char *argv[]) 
{ 

int L1,L2,L3,H1,H2,H3,E1,E2,E3; 
char *s="X M ; 



/♦positioning computing initialization*/ 
max-0; 

for( i=0;i<2;i++) /*find the two beacons which maximize the y[i]-y[j]*/ 

< 

for ( j=i+l; j<3; 
( 

diff=abs(y[i]-y[j]); 

if (diff>max) 

{ 

max=dif f ; 

m^i; 

n=j; 

k=3-i-j; 

) 

> 
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diff=y[m]-y[n) ; 
B=(x[m]-x[n] )/diff; 
u=l+sqr (B) ; 

/*end positioning initialization*/ 

lnp_logical_range ( 1) ; /*set long range*/ 

/*start the IR / ultrasonic receiver*/ 
motor_a_dir (fwd) ; 
irtotor_a_speed(MAX_SPEED) ; 
cputs ("wait" ) ; 
sleep (6) ; 



while (1) /*repeat forever*/ 
{ 



s[0}='A' ; 

lnp_integrity__write (s, strlen (s) ) ; /*command beacon A to ping*/ 
msleep (40) ; /*wait until PIC no more in error-condition*/ 
El=receive ( ) ; /*get the error message*/ 
Ll=LOW__read; /*get the byte*/ 
Hi— HIGH read; 



msleep (25); /*recover-time*/ 



sfOJ-'B' ; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon B to ping*/ 
msleep (40); /*wait until PIC no more in error-condition*/ 
E2=receive ( ) ; /*get the error message*/ 
L2=LOW_read; /*get the byte*/ 
H2=HIGH_read; 



msleep (25); /*recover-time*/ 



s[0] = 'C ; 

lnp__integrity__write (s, strlen (s) ) ; /*command beacon C to ping*/ 
msleep (40) ; /*wait until PIC no more in error-condition*/ 
E3=receive ( ) ; /*get the error message*/ 
L3=L0W_read; /*get the byte*/ 
H3=HIGH read; 



lcd_int (El) ; 
sleep (1) ; 

lcd_int (distance (LI, Hi) ) ; 
sleep (1) ; 



lcd_int (E2) ; 
sleep (1) ; 

lcd_int (distance (L2,H2) ) ; 
sleep (1) ; 



lcd_int (E3) ; 
sleep (1 ) ; 

lcd__int (distance (L3,H3) } ; 
sleep (1) ; 



d[0]=distance(Ll,Hl) ; 
d[l]=distance(L2,H2) ; 
d[2]=distance(L3,H3) ; 
if (El+E2+E3==0) pos(); /*if there is no error, position the robot*/ 

) 

return 0; /*to get rid of compiler warning*/ 

) 
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Here now' the complete robot-prd^m with positioning and sending of^nessage to the beacons in order 
adjust there orientations. Note the multi-tasking with a new -still dummy- function called PILOT. This funct 
should be used for navigating the robot. This REC_BOT7.c should be used together with the A_4.c, B_4.c 
and C_4.c from the turnable beacons section . The correct beacon coordinates must be entered in red 
marked line. Place the beacons at the correct coordinates, then turn them to direction beta=0° (sense of th 
y-axle); start the beacons-programs. Wait the beacons to turn to the initial-positions. Turn on the receiver- 
task. Move the receiver very slowly around. The displaying slows down the process. 



/*rec_bot7 . c: should send a message to each beacon in order to PING 
Then calculate and display the values plus the positioning-calculations 
NEW NEW NEW NEW sends the P-message to the beacons and the coordinates 
NEW NEW NEW NEW multi-thread: new pilot-task and positioning-task*/ 

ttinclude <unistd.h> 
#include <conio.h> 
ttinclude <dmotor.h> 
#include <dsensor.h> 
#include <string.h> 
#include <lnp.h> 
#include <lnp-logical . h> 

pid_t position_thread; 
pid_t pilot_thread; 

/*++++++++++++++++++begin receive routines+++++++++++++*/ 

#define aa .0710843 /^linear function to transform raw_l to real half-byte*/ 
#define bb -6.9802983 

#define cycle 4.3904 /*=1.28E-4 * 343 * 100 result in cm*/ 

int LOW_read, HIGH_read; 

int receive ( ) 
{ 

int idx,wdt,err; 
long int SUM; 

^initialize main variables*/ 

LOW_read=0; 

HIGH_read=0; 

err=0; /*no error*/ 

wdt=0; /*watch-dog-timer*/ 

/*communication-protocole*/ 



while (SENSOR_1>16000) /*wait until start-message*/ 
{ 

wdt+=l; 

if (wdt>3000) { err=+l; break; } 

} 

if {err==0) /*continue if no error*/ 
{ 

wdt=0; 

while (SENSOR_1<16001) /*wait until low_byte*/ 
< 

wdt+=l; 

if (wdt>1000) { err=+2; break; } 

) 

} 

/*while waiting until pause-message compute average*/ 
if (err==0) /*continue if no error*/ 

SUM=0; 
idx=0; 

while (SENSOR_1>16000) /*wait until high_byte*/ 
{ 

SUM+=SENSOR_l; 
idx+=l; 

if(idx>1000) { err=+3; break; J 

} 



LOW_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 
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if(err-O) /*continue if no error*/ 
i 

wdt=0; 

while (SENSOR_1<16001) 
{ 

wdt+=l; 

if (wdt>1000) { err=+4; break; } 

> 

} 

/*while waiting until stop-message compute average*/ 



if (err==0) /*continue if no error*/ 
{ 

SUM=0; 
idx=0 ; 

while (SENSOR_1>16000) 
{ 

SUM+=SENS0R_1; 
idx+=l; 

if (idx>1000) { err=+5; break; } 

} 



HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 

} 



if(err==0) /*continue if no error*/ 
{ 

wdt=0; 

while (SENSOR_1<16001) /*wait until NO_SIGNAL condition*/ 

{ 

wdt+=l; 

if (wdt>1000) { err=+6; break; } 

} 

) 

return err; 

} 



int distance (int L, int H) 
{ 

double raw_l,dist; 
int LOW, HIGH, result; 
/*convert raw sensor-values to half-byte values*/ 
raw_l=100000/L; 
r aw_l=aa * r aw_l +bb ; 
LOW=raw_l ; 
raw_l=100000/H; 
raw_l=aa*raw_l+bb; 
HIGH=raw_l; 

/*calculate the distance*/ 
dist=cycle* (16*HIGH-t-LOW) + . 5; 
result=dist; /*double to integer*/ 

return (dist) ; 



/*++++++++++++++++end receive routines+++++++++++++++V 



/*++++++++++++++begin positioning routines++++++++++++*/ 



double abs (double x) /*abs value-function*/ 
{ 

if{x>0) {return x; } else { return -x; } 

> 



double sqr (double x) /*square-function*/ 
{ 

return x*x; 

} 

double sqrt (double x) /*square-root-function*/ 
{ /*Heron's algorithm*/ 

int idx; 
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double q,y; 
q=0; 
y=2; 

for (idx=0;idx<14;idx++) /*enough precision and prevent 

program from getting lost in loop*/ 

< 

q=x/y; 
y=(y+q)/2; 

} 

return y; 



#define POINTS 3 
#define SOLUTIONS 2 

tfdefine TOLERANCE 2500 /*this is the square of the maximum distance of 

the two-circle intersection solution point to the 
third circle*/ 



int x[POINTS]={0, 92, 0} ; /*make these variables giobal<====™=========beacon coordinate 3==============*/ 

int y[POINTS]={0, 75, 150} ; /*beacon-positions in cm*/ 

int d[POINTS]={0,0,0); 

int i, j , k,m, n, dif f ,max; 

double B,u; 

double xr [SOLUTIONS] ; 

double yr [SOLUTIONS] ; 



int pos ( ) 
{ 



int tempi, err; 

double A, v, w, sq_d, delta, temp2, tl, t2; 



err=0; /*no error yet*/ 



/*now the calculations*/ 

A={ (sqr (d[n] )-sqr (d[m] )+sqr (x[m] ) -sqr (x[n] ) ) /dif f +y [m] +y [n] ) /2; 



v=2* (y[n] *B-x[n]-A*B) ; 

w=-sqr (d[n] )+sqr (x[n] )+sqr (y[n] )+sqr(A) -2*y[n] *A; 
delta=-4*u*w+sqr (v) ; 
if(delta<0) {err=7; } else 
{ 

sq_d=sqrt (delta) ; 
xr[0]=(sq_d-v) /u/2; 
xr[l]=-(v+sq_d)/u/2; 
yr[0]=A-B*xr[0] ; 
yr[l]=A-B*xr[l] ; 



templ=xr [0] ; 
cputs ("xrO" ) ; 
sleep (1) ; 
lcd_int (tempi) ; 
sleep (1) ; 



templ=yr [0] ; 
cputs ("yrO" ) ; 
sleep (1) ; 
lcd_int (tempi) ; 
sleep (1) ; 



templ=xr [1] ; 
cputs ( "xrl" ) ; 
sleep (1) ; 
lcd_int (tempi) ; 
sleep (1) ; 



templ=yr [1] ; 
cputs ("yrl" ) ; 
sleep (1) ; 
lcd_int (tempi) ; 
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sleep (1); 

tl=abs(sqr(x[k]-xr[0])+sqr(y[kJ-yr[O])-sqr{d[k])); /*test the third circle*/ 
t2=abs(sqr(x[k]-xr[l])+sqr(y[k]-yr[l])-sqr(d[k] ) ) ; 



if (t2<tl) 
{ 



/^exchange the solution, indexO will point the true solution*/ 

temp2=xr [0J ; 
xr[0]=xr[l] ; 
xr [l]=temp2; 

temp2=yr [0} ; 
yr[0]=yr[l]; 
yr [ 1 ] =temp2 ; 

temp2=t2; 

t2=tl; 
tl=temp2; 



} 

if (t INTOLERANCE) err=8; 



return err; 

> 

void P__message() 
{ 

char *st="Pms"; 
int x,y; 

x=xr[0]/6; /*this precision is enough for the beacons to fix the robot*/ 

if (x>127) x=127; /*value still signed */ 

if (x<-128) x=-128; 
x+-128; /*to make an unsigned char*/ 
y=yr [0] /6; 
if (y>127) y=127; 

if (y<-128) y=-128; 
y+=128; 
st[l]=x; 
st[2]=y; 

lnp_integrity_write (st, strlen (st) ) ; /*send generally P-message*/ 
msleep{40); /*wait PIC error-condition to recover*/ 

> 

/*++++++++++++++++++End Positioning routines++++++++++*/ 
/*+++++++++++++++Begin Positioning task+++++++++++++++*/ 

int position () 

int Ll,L2,L3,Hl,H2,H3,El,E2,E3,error; 
char *s="X"; 



/*positioning initialization*/ 
max=0 ; 

for( i=0; i<2; i++) /*find the two beacons which maximize the y[i]-y[j]V 

{ 

for ( j=i+l; j<3; j++) 
{ 

diff=abs(y[i]-y[j]); 
if (dif f>max) 
{ 

max=dif f ; 
m=i; 
n=j; 

k=3-i-j; 

} 

} 

} 
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B=(x[ra]-x[n] )/diff; 
u=l+sqr (B) ; 



/*end positioning initialization*/ 

while (1) /*repeat forever*/ 
{ 



s[0]='A'; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon A to ping*/ 
msleep(40); /*wait until PIC no more in error-condition*/ 
El=receive ( ) ; /*get the error message*/ 
Ll=LOW_read; /*get the byte*/ 
H1=HIGH read; 



msleep (25) ; /*recover-time*/ 



s[0]='B'; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon B to ping*/ 
msleep ( 40) ; /*wait until PIC no more in error-condition*/ 
E2=receive ( ) ; /*get the error message*/ 
L2=LOW_read; /*get the byte*/ 
H2=HIGH read; 



msleep (25); /*recover-time*/ 



s[0]-'C' ; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon C to ping*/ 
msleep(40); /*wait until PIC no more in error-condition*/ 
E3=receive ( ) ; /*get the error message*/ 
L3=LOW_read; /*get the byte*/ 
H3=HIGH read; 



lcd_int (El) ; 
sleep (1) ; 

lcd_int (distance (LI, HI) ) ; 
sleep (1) ; 



lcd_int (E2) ; 
sleep( 1) ; 

lcd_int (distance (L2,H2) ) ; 
sleep (1) ; 



lcd_int (E3) ; 
sleep (1) ; 

lcd_int (distance (L3, H3) ) ; 
sleep (1) ; 



d[0]=distance(Ll,Hl) ; 
d[l]=distance(L2,H2) ; 
d[2)=distance{L3,H3) ; 
if (El+E2+E3==0) /*if there is no error, position the robot*/ 
{ 

error=pos ( ) ; 

if (error==0) ( P_message(); } 
else 

( 

cputs ("err") ; 
sleep (1) ; 
lcd_int (error) ; 
sleep (1) ; 

} 

} 

} 

return 0; /*to get rid of the compiler messages*/ 

> 
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/*+++++4++++++++++End Positioning tasJ^B^++++++++++++*/ 





/*+++++++++++++++Begin Pilot routines++++-t-++++++++++++*/ 

/*++++++++++++++++End Pilot routines++++++++++++++++++*/ 

/*+++++++++++++++Begin Pilot task++++++++++-*-+++++++++++*/ 

int pilot () 

< 



/*MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ 



int main (int argc, char *argv[] ) 
{ 



lnp_logical_range (1) ; /*set long range*/ 



/*start the IR / ultrasonic receiver*/ 
motor_a_dir (fwd) ; 
motor_a_speed(MAX_SPEED) ; 
cputs ("wait") ; 
sleep (6) ; 

position_thread=execi (&position, 0,0, PRIO_NORMAL+5, DEFAULT_STACK_SIZE) ; 
pilot_thread=execi (fipilot, 0, 0, PRIO_NORMAL, DEFAULT_STACK_SIZE) ; 



return 0; /*to get rid of compiler warning*/ 

} 



Note that error messages AND echo values are pruned. So if such a condition is generated somewhere, 
there will be no new positioning! 

If the robot gets out-of-range errors from one beacon, in a future program, the concerned beacon may be 
ordered do sweep for finding th robot again. The same might be done if echo-values occured (recognizabl 
by error-messages 7 or 8!!!). In this case however, all the beacons should sweep, because there is no tes 
yet, which beacon caused the echo value. 

HERE NOW THE FIRST REAL-TIME TEST PROGRAM: rec_bot8.c ! Use with A_5.c , B_5.c and C_5.c ! 
There is no display at all. Proceed as for the previous program. Yeah!!! It works! 



/*rec_bot8 . c : should send a message to each beacon in order to PING 
Then calculate the distance-values plus the positioning-calculations 
NEW NEW NEW NEW sends the P-message to the beacons and the coordinates 
NEW NEW NEW NEW multi-thread: new pilot-task and positioning-task 
+ some corrections NEW no display*/ 

#include <unistd.h> 



#include <dmotor.h> 
#include <dsensor.h> 
#include <string.h> 
#include <lnp.h> 
#include <lnp-logical . h> 

pid_t position_thread; 
pid_t pilot_thread; 



/*do nothing yet but loop*/ 
while (1) msleep(lO) ; 



return 0; 
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/*++++-++++++++++-n-++begin receive routines++++-+++++++++*/ 

tdefine aa .0710843 /^linear function to transform raw_l to real half-byte*/ 
#define bb -6.9802983 

#define cycle 4.3904 /*«1.28E-4 * 343 * 100 result in cm*/ 

int LOW_read,HIGH_read; 

int receive ( ) 
( 

int xdx, wdt, err ; 
long int SUM; 

/*initialize main variables*/ 

LOW_read=0; 

HIGH_read=0; 

err^O; /*no error*/ 

wdt=0; /*watch-dog-timer*/ 

/ *communication-protocole* / 



while (SENSOR_1>16000) /*wait until start-message*/ 
{ 

wdt+=l; 

if (wdt>3000) { err=+l; break; } 

} 

if (err— 0) /*continue if no error*/ 
{ 

wdt^O; 

while (SENSOR_1<16001) /*wait until low_byte*/ 
{ 

wdt+=l ; 

if (wdt>1000) { err=+2; break; } 

} 

) 

/*while waiting until pause-message compute average*/ 
if(err=«0) /*continue if no error*/ 

{ 

SUM=0; 
idx=0; 

while (SENSOR_1>16000) /*wait until high_byte*/ 
{ 

S0M+=SENSOR_l; 
idx+=l; 

if(idx>1000) { err=+3; break; ) 

J 



LOW_read~SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 

) 



if (err— 0) /*continue if no error*/ 
{ 

wdt«0 ; 

while (SENSOR_1<16001) 
( 

wdt+=l; 

if (wdt>1000) f err=+4; break; } 

} 

) 

/*while waiting until stop-message compute average*/ 

if(err«-0) /*continue if no error*/ 
{ 

SUM=0 ; 
idx=0; 

while (SENSOR_1>16000) 
{ 

SUM+=SENSOR_l; 
idx+=l ; 

if(idx>1000) { err^+5; break; } 

> 



HIGH_read=SUM/64/idx; /*gives normal RCX raw-values 0..1023*/ 

} 
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v \ 

if (err==0) /*continue if no error*/ 
{ 

wdt-0; 

while (SENSOR_1<16001) /*wait until N0_SIGNAL condition*/ 
{ 

wdt+=l; 

if (wdt>1000) { err=+6; break; } 

) 

} 

return err; 



int distance (int L, int H) 
t 

double raw__l,dist; 
int LOW, HIGH, result; 
/♦convert raw sensor-values to half-byte values*/ 
raw_l=100000/L; 
raw_l=aa*raw_l+bb; 
L0W=raw_l ; 
raw_l=100000/H; 
raw_l=aa*raw_l+bb ; 
HIGH=raw_l; 



/^calculate the distance*/ 
dist=cycle* (16*HIGH+L0W) + . 5; 
result=dist; /^double to integer*/ 

return(dist) ; 



/*++++++++++++++++end receive routines+++++++++++++++V 



/*++++++++++++++begin positioning routines++++++++++++*/ 



double abs (double x) /*abs value-function*/ 
f 

if(x>0) {return x; } else { return -x; } 

} 



double sqr {double x) /*square-function*/ 
I 

return x*x; 

} 



double sqrt (double x) /*square-root-function*/ 
( /*Heron's algorithm*/ 

int idx; 

double q,y; 

q=0; 

y=2; 

for (idx=0;idx<14;idx++) /*enough precision and prevent 

program from getting lost in loop*/ 

{ 

q-x/y; 
y=(y+q)/2; 

} 

return y; 

} 



#define POINTS 3 
#define SOLUTIONS 2 

#define TOLERANCE 2500 /*this is the square of the maximum distance of 

the two-circle intersection solution point to the 
third circle*/ 



int x [POINTS] ={0, 92, 0} ; /*make these variables global <===============beacon coordinate s==============*/ 

int y[POINTS]«{ 0,75, 150) ; /*beacon-positions in cm*/ 

int d[POINTS]={0, 0, 0} ; 

int i, j , k, m, n, dif f , max; 

double B,u; 

double xr [SOLUTIONS] ; 

double yr [SOLUTIONS] ; 

int pos ( ) 

{ 
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int tempi, err; 

double A, v, w, sq_d, delta, temp2, tl, t2; 



err=0; /*no error yet*/ 



/*now the calculations*/ 

A=( (sqr(d[nj )-sqr(d[ra] )+sqr(x[m] )-sqr(x[n] ) ) /diff +y[ra] +y (n] ) /2; 



v=2* (y [n] *B-x[n] -A*B) ; 

w=-sqr (d[n] )+sqr(x[n] ) +sqr (y[nj )+sqr (A)-2*y[nJ *A; 
delta=-4*u*w+sqr (v) ; 
if(delta<0) {err=7; } else 
{ 

sq_d=sqrt (delta) ; 
xr[0]={sq_d-v)/u/2; 
xr[l]=-(v+sq_d)/u/2; 
yr [0]=A-B*xr [0] ; 
yr [1]=A-B*xr [1] ; 



tl=abs (sqr (x[k] -xr [0] ) +sqr (y [k] -yr [0] )-sqr (d[k] ) ) ; /*test the third circle*/ 
t2=abs(sqr (x[k] -xr [1] ) +sqr (y [k] -yr [ 1] )-sqr(d[k] )) ; 



if (t2<tl) 
{ 



temp2=t2; 



} 



/^exchange the solution, indexO will point the true solution*/ 

temp2=xr [0] ; 
xr[0]=xr[l] ; 
xr [l]=temp2; 

temp2=yr [0] ; 
yr[0]=yr[l] ; 
yr [ 1] = temp 2 ; 



t2=tl; 
tl=temp2; 



if (tl>TOLERANCE) err=8; 



return err; 



void P_message() 



char *st= ,, Pms" ; 
int x,y; 

x=xr[0J/6; /*this precision is enough for the beacons to fix the robot*/ 

if (x>127) x=127; /*value still signed */ 

if (x<-128) x=-128; 
x+=128; /*to make an unsigned char*/ 
y=yr[0]/6; 
if (y>127) y=127; 

if (y<-128) y=-128; 
y+~128; 
st [lj=x; 
st[2]=y; 

lnp_integrity_write (st, strlen(st) ) ; /*send generally p-message*/ 
msleep(40); /*wait PIC error-condition to recover*/ 



/*++-f+++++++++++++++End Positioning routines++++++++++*/ 
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int position*) 
{ 

int L1,L2, L3, Hi, H2, H3, El, E2, E3, error; 
char *s="X"; 



/*positioning initialization*/ 
max=0; 

for( i=0; i<2; i++) /*find the two beacons which maximize the y [ i] -y [ j ] */ 

{ 



dif f =y [m] -y [n] ; 
B=(x[m]-x[n] )/diff ; 
u=l+sqr (B) ; 



/*end positioning initialization*/ 



while (1) /*repeat forever*/ 
{ 



lnp_integrity_write (s, strlen (s) ) ; /*command beacon A to ping*/ 
msleep (40); /*wait until PIC no more in error-condition*/ 
El=receive ( ) ; /*get the error message*/ 
Ll=LOW_read; /*get the byte*/ 
Hl=HIGH_read; 



msleep{25); /*recover-time*/ 



s[0]= , B'; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon B to ping*/ 
msleep (40) ; /*wait until PIC no more in error-condition*/ 
E2=receive ( ) ; /*get the error message*/ 
L2=LOW_read; /*get the byte*/ 
H2=HIGH read; 



msleep (25); /*recover-time*/ 



st03='C'; 

lnp_integrity_write (s, strlen (s) ) ; /*command beacon C to ping*/ 
msleep (40); /*wait until PIC no more in error-condition*/ 
E3=receive ( ) ; /*get the error message*/ 
L3~LOW__read; /*get the byte*/ 
H3=HIGH read; 



msleep (25); /*recover-time*/ 



d[0]=distance(Ll,Hl) ; 
dnj=distance(L2 / H2) ; 
d[2]=distance(L3,H3) ; 
if (El+E2+E3=*=*0) /*if there is no error, position the robot*/ 



for < j<3; 



diff=abs(y[i]-y[j]); 
if (diff>max) 



max=dif f ; 

m-i; 

n=j; 

k=3-i-j; 



error=pos () ; 

if (error==0) ( P_message{); } 
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/*do nothing*/ 



msleep ( 40) ; /* recover- time*/ 



return 0; /*to get rid of the compiler messages*/ 



/*++++++++++++++++End Positioning task++++++++++++++++*/ 



/*+++++++-j-+++++++Begin Pilot routines+++++++++++++++++*/ 



/*++++++++++++++++End Pilot routines 




/*+++++++++++++++Begin Pilot task+++++++-+++-f+++++++++-f+*/ 
int pilot () 



/*do nothing yet but loop*/ 
while (1) msleep (10); 



return 0; 



/*++++++++++++++++End Pilot task+++++++++++++++++++++-f+*/ 



/♦MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN*/ 



int main(int argc, char *argv[] ) 
{ 



lnp_logical_range { 1) ; /*set long range*/ 

/*start the IR / ultrasonic receiver*/ 
motor_a_dir (fwd) ; 
motor_a_speed(MAX_SPEED) ; 



sleep (6) ; 

posit ion_thread=execi (Sposition, 0,0, PRI0_NORMAL+5 , DEFAULT_STACK_SIZE) ; 
pilot_thread=execi ( fipilot ,0,0, PRIO_NORMAL, DEFAULT_STACK_SIZE) ; 



return 0; /*to get rid of compiler warning*/ 

) 



^i Main Page 
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